#include <stdio.h>
#include <stdarg.h>
#include <time.h>
#include <unistd.h>
#include <sys/time.h>

#include "mirrorDriver.h"

static char ip_address[] = "192.168.0.1";

static const double pi = 3.14159265358979323846;

static char *
timestamp(void)
{
  static char s[] = "YYYY-MM-DD HH:MM:SS";
  time_t t = time(NULL);
  struct tm *tm = localtime(&t);  
  strftime(s, sizeof(s), "%F %T", tm);
  return s;
}

static void
log(const char *message, ...)
{
  va_list ap;
  va_start(ap, message);
  fprintf(stderr, "%s: ", timestamp());
  vfprintf(stderr, message, ap);
  fprintf(stderr, "\n");
  va_end(ap);
}

static void
error(const char *message, ...)
{
  va_list ap;
  va_start(ap, message);
  fprintf(stderr, "%s: error: ", timestamp());
  vfprintf(stderr, message, ap);
  fprintf(stderr, "\n");
  va_end(ap);
  exit(1);
}

static double
currentsecond(void)
{
  struct timeval tv;
  gettimeofday(&tv, NULL);
  return (double) tv.tv_sec + 1e-6 * (double) tv.tv_usec;
}

static double
sinewaveform(double phi)
{
  return sin(phi);
}

static double
squarewaveform(double phi)
{
  if (phi < pi)
    return +1.0;
  else
    return -1.0;
}

int
main(int argc, const char *argv[])
{

  log("starting.");
  
  // Close stdout, as the mirror driver library writes lots of messages
  // to stdout.
  fclose(stdout);
  
  if (argc != 5)
    error("arguments are: time amplitude frequency waveform");
  double T = strtod(argv[1], NULL);
  double amplitude = strtod(argv[2], NULL);
  double nu = strtod(argv[3], NULL);
  const char *waveformname = argv[4];
  
  log("driving for %.1f s.", T);
  log("driving with a %s wave with amplitude %.3f and frequency %.0f Hz.", 
    waveformname, amplitude, nu);
  
  double (*waveform)(double);
  if (strcmp(waveformname, "sine") == 0)
    waveform = sinewaveform;
  else if (strcmp(waveformname, "square") == 0)
    waveform = squarewaveform;
  else
    error("unsupported wave form \"%s\".", waveformname);

  mirrorDriver *m = new mirrorDriver();
  
  log("connecting to the mirror at %s.", ip_address);
  if (!m->configIPAddress(ip_address))
    error("unable to configure the mirror IP address.");
  if (!m->connectToMirror())
    error("unable to connect to the mirror.");

  int n = m->getNumMirrorChannels();
  log("mirror has %d channels.", n);
  
  log("starting driving.");
  double start = currentsecond();
  long i = 0;
  
  for (;;) {
  
    double t = currentsecond() - start;
    if (t > T)
      break;
    ++i;

    static float v0[] = {
      // Convex defocus.
      0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00,
      1.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00,
      0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00,
      0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00,
      0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00,
      0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00,
      0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00,
      0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00,
    };

    static float v1[] = {
      // Concave defocus.
      0.00, 0.00, 0.00, 1.00, 0.00, 0.00, 0.00, 1.00,
      0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00,
      0.00, 0.00, 0.00, 1.00, 0.00, 0.00, 0.00, 1.00,
      1.00, 0.00, 0.00, 1.00, 1.00, 0.00, 1.00, 1.00,
      0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00,
      0.00, 0.00, 1.00, 0.00, 0.00, 0.00, 1.00, 0.00,
      1.00, 1.00, 0.00, 0.00, 1.00, 1.00, 0.00, 0.00,
      1.00, 0.00, 0.00, 0.00, 1.00, 0.00, 0.00, 0.00,
    };

    float v[n];
    double phi = 2.0 * pi * (t * nu - floor(t * nu));
    double f = waveform(phi);
    for (int j = 0; j < n; ++j) {
      v[j] = 0.5 + amplitude * f * v0[j];
    }

    if (!m->setChannels(v)) {
      error("unable to set voltages.");
    }
    
  }

  double end = currentsecond();
  log("finished driving.");
  
  log("achieved %.0f Hz.", (double) i / (end - start));
  log("%.0f µs per iteration.", (end - start) / (double) i * 1e6);

  log("disconnecting from the mirror.");
  if (!m->disconnect())
    error("disconnecting failed.");
    
  log("exiting.");
  return 0;
}
